﻿using UnityEngine;
using System;
using System.Linq;
using System.Collections;
using System.Collections.Generic;

using Hont;

namespace Hont.AStar
{
    public class HontAStarUnity : MonoBehaviour
    {
        public const string OCTTREEUSERDATA_HAS_WALKABLE_BOX = "HasWalkableBox";

        static Vector3 mLambda_CacheComparePos;

        static List<HontAStarUnity> mCreatedAStarList = new List<HontAStarUnity>();
        public static List<HontAStarUnity> CreatedAStarList { get { return mCreatedAStarList; } }

        [Serializable]
        public class AdvanceSetting
        {
            public MonoBehaviour customHCost;
            public int straightCost = 10;
            public int diagonalCost = 15;
            public int octTreeDepth = 2;
            public bool buildOctTree = true;
            [Tooltip("'Awake' init or 'Start' init.")]
            public bool awakeInit = false;
            public bool drawGizmos = false;
        }

        [Serializable]
        public class GizmosSetting
        {
            public SoloTypeEnum soloDisplayType;
            [Range(0, 1)]
            public float alpha = 0.4f;
            public bool enableSizeSolo;
            public Vector2 soloLengthRange;
            public Vector2 soloWidthRange;
            public Vector2 soloHeightRange;
        }

        public enum SoloTypeEnum { None, Walkable, Obstacle, Path, Mask, Cost, CenterPoint, All }
        public int lenght = 10;
        public int width = 10;
        public int height = 1;
        public int defaultCost = 14;
        public float mappingLengthSize = 24;
        public float mappingWidthSize = 24;
        public float mappingHeightSize = 24;
        public HontAStarUnityPersistData astarData;
        public bool autoLoadAStarData = true;
        public AdvanceSetting advanceSetting;
        public GizmosSetting gizmosSetting;
        HontAStar mAStar;
        HontOCTTree<Position> mOctTree;

        public HontOCTTree<Position> OctTree { get { return mOctTree; } }
        public bool IsInitialized { get { return mAStar != null; } }
        public Grid Grid { get { return mAStar == null ? null : mAStar.Grid; } }
        public int TotalSize { get { return lenght * width * height; } }
        public Vector3 MappingSize { get { return new Vector3(mappingLengthSize, mappingHeightSize, mappingWidthSize); } }
        public IAStarStepDebuger Debuger
        {
            get
            {
                if (mAStar == null) return null;
                else return mAStar.Debuger;
            }
            set
            {
                if (mAStar == null) return;
                mAStar.Debuger = value;
            }
        }

        public Bounds LocalBounds
        {
            get
            {
                var boundsSize = new Vector3(lenght * mappingLengthSize, height * mappingHeightSize, width * mappingWidthSize);

                return new Bounds { center = boundsSize * 0.5f, size = boundsSize };
            }
        }

        public Bounds Bounds
        {
            get
            {
                return new Bounds { center = transform.position + LocalBounds.center, size = LocalBounds.size };
            }
        }


        void Awake()
        {
            if (!advanceSetting.awakeInit) return;
            if (!autoLoadAStarData) return;

            Init();
        }

        void Start()
        {
            if (advanceSetting.awakeInit) return;
            if (!autoLoadAStarData) return;

            Init();
        }

        void OnDestroy()
        {
            mCreatedAStarList.Remove(this);
        }

        void OnValidate()
        {
            if (gizmosSetting == null) return;

            var vector = gizmosSetting.soloLengthRange;
            vector.x = Mathf.Floor(gizmosSetting.soloLengthRange.x);
            vector.y = Mathf.Floor(gizmosSetting.soloLengthRange.y);
            vector.x = Mathf.Max(vector.x, 1);
            vector.y = Mathf.Clamp(vector.y, vector.x, lenght);
            gizmosSetting.soloLengthRange = vector;

            vector = gizmosSetting.soloWidthRange;
            vector.x = Mathf.Floor(gizmosSetting.soloWidthRange.x);
            vector.y = Mathf.Floor(gizmosSetting.soloWidthRange.y);
            vector.x = Mathf.Max(vector.x, 1);
            vector.y = Mathf.Clamp(vector.y, vector.x, width);
            gizmosSetting.soloWidthRange = vector;

            vector = gizmosSetting.soloHeightRange;
            vector.x = Mathf.Floor(gizmosSetting.soloHeightRange.x);
            vector.y = Mathf.Floor(gizmosSetting.soloHeightRange.y);
            vector.x = Mathf.Max(vector.x, 1);
            vector.y = Mathf.Clamp(vector.y, vector.x, height);
            gizmosSetting.soloHeightRange = vector;
        }

        void OnDrawGizmos()
        {
            if (advanceSetting == null || !advanceSetting.drawGizmos) return;

            var oldColor = Gizmos.color;
            var oldMatrix = Gizmos.matrix;

            Gizmos.matrix = transform.localToWorldMatrix;

            Gizmos.color = new Color(1, 1, 1, 0.5f);
            Gizmos.DrawWireCube(LocalBounds.center, LocalBounds.size);

            var arrowMinVector = LocalBounds.min;
            var arrowMaxVector = LocalBounds.max;

            var xMax = new Vector3(arrowMaxVector.x, arrowMinVector.y, arrowMinVector.z);
            var yMax = new Vector3(arrowMinVector.x, arrowMaxVector.y, arrowMinVector.z);
            var zMax = new Vector3(arrowMinVector.x, arrowMinVector.y, arrowMaxVector.z);

# if UNITY_EDITOR

            if (UnityEditor.Selection.gameObjects.Length > 1 && UnityEditor.Selection.gameObjects.Contains(gameObject))
            {
                var list = UnityEditor.Selection.gameObjects.ToList();
                list.Remove(gameObject);

                UnityEditor.Selection.objects = list.ToArray();
            }

            UnityEditor.Handles.Label(transform.TransformPoint(xMax), "Length(x)");
            UnityEditor.Handles.Label(transform.TransformPoint(yMax), "Height(y)");
            UnityEditor.Handles.Label(transform.TransformPoint(zMax), "Width(z)");
#endif
            //X Arrow
            Gizmos.DrawLine(arrowMinVector, xMax);
            //Y Arrow
            Gizmos.DrawLine(arrowMinVector, yMax);
            //Z Arrow
            Gizmos.DrawLine(arrowMinVector, zMax);

            Gizmos.color = oldColor;
            Gizmos.matrix = oldMatrix;
        }

        public void BuildOctTree()
        {
            BuildOctTree(m => (m as HontAStarUnityNode).MappingPos);
        }

        public void BuildOctTree(Func<object, Vector3> userDataToCenterPos)
        {
            mOctTree = new HontOCTTree<Position>(LocalBounds, advanceSetting.octTreeDepth, new HontAStarUnityOCTTreeSyncer(this));

            foreach (Position item in Grid)
            {
                var centerPos = userDataToCenterPos(Grid.GetUserData(item));

                mOctTree.Root.AddItem(new GeneralOCTItem<Position>() { Position = centerPos, Value = item });
            }
        }

        /// <summary>
        /// Initialize astar data, invoke in runtime or editor mode.
        /// </summary>
        public void InitAStarData(Grid grid)
        {
            var hCost = advanceSetting.customHCost as IAStarHCost;
            if (hCost == null)
                hCost = new EasyAStarHCost();

            mAStar = new HontAStar(hCost, advanceSetting.straightCost, advanceSetting.diagonalCost);
            mAStar.Init(grid);
        }

        public Grid LoadAStarData(HontAStarUnityPersistData astarData)
        {
            return LoadAStarData(astarData, m => new HontAStarUnityNode(this, m));
        }

        public Grid LoadAStarData(HontAStarUnityPersistData astarData, Func<HontAStarUnityPersistData.Node, object> setUserdataFunc)
        {
            this.astarData = astarData;

            lenght = astarData.lenght;
            width = astarData.width;
            height = astarData.height;
            mappingLengthSize = astarData.mappingLengthSize;
            mappingWidthSize = astarData.mappingWidthSize;
            mappingHeightSize = astarData.mappingHeightSize;

            var result = new Grid(lenght, width, height);

            for (int i = 0; i < astarData.nodeArr.Length; i++)
            {
                var item = astarData.nodeArr[i];
                result.SetCost(new Position(item.X, item.Y, item.Z), item.Cost);
                result.SetMask(new Position(item.X, item.Y, item.Z), item.Mask);
                result.SetIsWalkable(new Position(item.X, item.Y, item.Z), item.IsWalkable);
                result.SetUserData(new Position(item.X, item.Y, item.Z), setUserdataFunc(item));
            }

            return result;
        }

        public Vector3[] StartPathfinding(Vector3 beginPosition, Vector3 endPosition, int mask = -1)
        {
            return StartPathfinding(beginPosition, endPosition, m => (m as HontAStarUnityNode).MappingPos, m => (m as HontAStarUnityNode).CustomMappingPos, mask);
        }

        public Vector3[] StartPathfinding(Vector3 beginPosition, Vector3 endPosition, Func<object, Vector3> userDataToCenterPosition, Func<object, Vector3> userDataToOutputPosition, int mask = -1, bool combinePath = true)
        {
            Func<Vector3, Bounds> posToBounds = (pos) => new Bounds(pos, MappingSize);

            var beginAStarPos = default(Position);
            var endAStarPos = default(Position);

            var convBeginPos = transform.InverseTransformPoint(beginPosition);
            var convEndPos = transform.InverseTransformPoint(endPosition);

            var nearestBeginPos = HontAStarUnityHelper.MatchToWalkableNearestPoint(this, convBeginPos);
            var nearestEndPos = HontAStarUnityHelper.MatchToWalkableNearestPoint(this, convEndPos);

            if (mOctTree != null)
            {
                var beginAStarOctItem = mOctTree
                    .Root
                    .GetItems(m => -Vector3.Distance(m.Bounds.center, nearestBeginPos))
                    .FirstOrDefault(m => posToBounds(userDataToCenterPosition(Grid.GetUserData(m.Value))).Contains(nearestBeginPos));

                var endAStarOctItem = mOctTree
                   .Root
                   .GetItems(m => -Vector3.Distance(m.Bounds.center, nearestEndPos))
                   .FirstOrDefault(m => posToBounds(userDataToCenterPosition(Grid.GetUserData(m.Value))).Contains(nearestEndPos));

                if (beginAStarOctItem == null || endAStarOctItem == null) return null;

                beginAStarPos = beginAStarOctItem.Value;
                endAStarPos = endAStarOctItem.Value;
            }
            else
            {
                beginAStarPos = mAStar.Grid.FirstOrDefault(m => posToBounds(userDataToCenterPosition(Grid.GetUserData(m))).Contains(nearestBeginPos));
                endAStarPos = mAStar.Grid.FirstOrDefault(m => posToBounds(userDataToCenterPosition(Grid.GetUserData(m))).Contains(nearestEndPos));
            }

            var pathfindingResult = StartPathfinding(beginAStarPos, endAStarPos, mask, combinePath);

            if (pathfindingResult == null) return null;

            var pathfindingVector3Result = Array.ConvertAll(pathfindingResult, m => userDataToOutputPosition(Grid.GetUserData(m)));
            pathfindingVector3Result = Array.ConvertAll(pathfindingVector3Result, m => transform.TransformPoint(m));

            return pathfindingVector3Result;
        }

        public Position[] StartPathfinding(Position beginAStarPosition, Position endAStarPosition, int mask = -1, bool combinePath = false)
        {
            var beginPosition = beginAStarPosition;
            var endPosition = endAStarPosition;

            return mAStar.Start(beginPosition, endPosition, mask, combinePath);
        }

        public int StartPathfinding_NonAlloc(Vector3 beginPosition, Vector3 endPosition, ref Position[] cachePositionArray, ref Vector3[] cacheResultArray, int mask = -1)
        {
            return StartPathfinding_NonAlloc(
                beginPosition,
                endPosition,
                m => (m as HontAStarUnityNode).MappingPos,
                m => (m as HontAStarUnityNode).CustomMappingPos,
                ref cachePositionArray,
                ref cacheResultArray,
                mask,
                true);
        }

        public int StartPathfinding_NonAlloc(Vector3 beginPosition, Vector3 endPosition, Func<object, Vector3> userDataToCenterPosition, Func<object, Vector3> userDataToOutputPosition, ref Position[] cachePositionArray, ref Vector3[] cacheResultArray, int mask = -1, bool combinePath = true)
        {
            var beginAStarPos = default(Position);
            var endAStarPos = default(Position);

            var convBeginPos = transform.InverseTransformPoint(beginPosition);
            var convEndPos = transform.InverseTransformPoint(endPosition);

            var nearestBeginPos = HontAStarUnityHelper.MatchToWalkableNearestPoint(this, convBeginPos);
            var nearestEndPos = HontAStarUnityHelper.MatchToWalkableNearestPoint(this, convEndPos);

            if (mOctTree != null)
            {
                mLambda_CacheComparePos = nearestBeginPos;

                var items = mOctTree
                    .Root
                    .GetItems(LambdaCache_GetOctTreeItems);

                var beginAStarOctItem = default(IOCTItem<Position>);

                for (int i = 0, iMax = items.Count; i < iMax; i++)
                {
                    var item = items[i];

                    var centerPosition = userDataToCenterPosition(Grid.GetUserData(item.Value));
                    var bounds = new Bounds(centerPosition, MappingSize);
                    if (bounds.Contains(nearestBeginPos))
                    {
                        beginAStarOctItem = item;
                        break;
                    }
                }

                mLambda_CacheComparePos = nearestEndPos;

                items = mOctTree
                   .Root
                   .GetItems(LambdaCache_GetOctTreeItems);

                var endAStarOctItem = default(IOCTItem<Position>);

                for (int i = 0, iMax = items.Count; i < iMax; i++)
                {
                    var item = items[i];

                    var centerPosition = userDataToCenterPosition(Grid.GetUserData(item.Value));
                    var bounds = new Bounds(centerPosition, MappingSize);
                    if (bounds.Contains(nearestEndPos))
                    {
                        endAStarOctItem = item;
                        break;
                    }
                }

                if (beginAStarOctItem == null || endAStarOctItem == null) return -1;

                beginAStarPos = beginAStarOctItem.Value;
                endAStarPos = endAStarOctItem.Value;
            }
            else
            {
                beginAStarPos = default(Position);

                using (var enumerable = (mAStar.Grid as IEnumerable<Position>).GetEnumerator())
                {
                    while (enumerable.MoveNext())
                    {
                        var item = enumerable.Current;

                        var centerPosition = userDataToCenterPosition(Grid.GetUserData(item));
                        var bounds = new Bounds(centerPosition, MappingSize);
                        if (bounds.Contains(nearestBeginPos))
                        {
                            beginAStarPos = item;
                            break;
                        }
                    }
                }

                endAStarPos = default(Position);

                using (var enumerable = (mAStar.Grid as IEnumerable<Position>).GetEnumerator())
                {
                    while (enumerable.MoveNext())
                    {
                        var item = enumerable.Current;

                        var centerPosition = userDataToCenterPosition(Grid.GetUserData(item));
                        var bounds = new Bounds(centerPosition, MappingSize);
                        if (bounds.Contains(nearestEndPos))
                        {
                            endAStarPos = item;
                            break;
                        }
                    }
                }
            }

            var length = StartPathfinding_NonAlloc(beginAStarPos, endAStarPos, ref cachePositionArray, mask, combinePath);

            if (length == -1) return -1;

            for (int i = 0, iMax = length; i < iMax; i++)
            {
                var item = cachePositionArray[i];
                var outputPos = userDataToOutputPosition(Grid.GetUserData(item));
                cacheResultArray[i] = transform.TransformPoint(outputPos);
            }

            return length;
        }

        public int StartPathfinding_NonAlloc(Position beginAStarPosition, Position endAStarPosition, ref Position[] cacheResultArray, int mask = -1, bool combinePath = true)
        {
            var beginPosition = beginAStarPosition;
            var endPosition = endAStarPosition;

            return mAStar.Start_NonAlloc(beginPosition, endPosition, ref cacheResultArray, mask, combinePath);
        }

        void Init()
        {
            mCreatedAStarList.Add(this);

            var grid = LoadAStarData(astarData);
            InitAStarData(grid);

            if (advanceSetting.buildOctTree)
                BuildOctTree();
        }
        
        static float LambdaCache_GetOctTreeItems(OCTNode<Position> arg)
        {
            return -Vector3.Distance(arg.Bounds.center, mLambda_CacheComparePos);
        }

        [ContextMenu("Debug Attributes")]
        void DebugAttributeInfo()
        {
            Debug.Log("OCT Tree: " + mOctTree + " LocalBounds: " + LocalBounds + " Bounds: " + Bounds);
        }
    }
}
